/*
 AnalogReadSerial
 Reads an analog input on pin 0, prints the result to the serial monitor 
 
 This example code is in the public domain.
 */

#include <GCS_MAVLink.h>

int packetDrops = 0;

void handleMessage(mavlink_message_t * msg) {
	Serial.print(", received mavlink message: ");
	Serial.print(msg->msgid, DEC);
}

void setup() {
	Serial.begin(57600);
	Serial3.begin(57600);
	mavlink_comm_0_port = &Serial3;
	packetDrops = 0;
}

void loop() {
	mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type,
			MAV_AUTOPILOT_ARDUPILOTMEGA);
	Serial.print("heartbeat sent");

	// receive new packets
	mavlink_message_t msg;
	mavlink_status_t status;

	Serial.print(", bytes available: ");
	Serial.print(comm_get_available(MAVLINK_COMM_0));
	while (comm_get_available( MAVLINK_COMM_0)) {
		uint8_t c = comm_receive_ch(MAVLINK_COMM_0);

		// Try to get a new message
		if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
			handleMessage(&msg);
	}

	// Update packet drops counter
	packetDrops += status.packet_rx_drop_count;

	Serial.print(", dropped packets: ");
	Serial.println(packetDrops);
	delay(1000);
}
